/********************************************************
* this file is for dynamics (CAN)
* author : liwang
* discription: 利用setget接口设置参数，设置过程注意需要清楚已完成的指令
* date : 20230609
*********************************************************/

#include "can_dynamics.h"
#include "errcode.h"
#include "modbuscrc.h"
#include "mot_priv.h"
#include "controlcan/controlcan.h"

// @FIXME liwang 适配7轴版本

static int setRobotPara(UINT8 joint, UINT16 cmd_type, UINT8 index, UINT32 para)
{
    return -1;
    // return setServoSDO(joint, cmd_type, 0x5D << 8 | index, para);
}

// 设置机器人动力学版本号
int setRobotDynamicsVersion(UINT8 joint, UINT32 para)
{
    return -1;
    // setRobotPara(joint, SET_ROBOT_DYNAMICS_VERSION, 0x01, para);
    return 0;
}

// 设置机器人CRC
int setRobotDynamicsCRC(UINT8 joint, UINT32 para)
{
    return -1;
    // setRobotPara(joint, SET_ROBOT_DYNAMICS_CRC, 0x00, para);
    // rtapi_print("Set CRC %d joint\n", joint);
    // return 0;
}

// 设置机器人动力学参数
int setRobotDynamicsPara(UINT8 joint, UINT8 index, UINT32 para)
{
    return -1;
    // UINT16 cmd_type = SET_ROBOT_DYNAMICS_PARA1 + index - 2;  //
    // setRobotPara(joint, cmd_type, index, para);
    return 0;
}

//
void setServoDynamics(int power_on, int enbale, const int* servo_dynamics_para, int dynamic_version, short* const dynamics_identifying_status)
{
#if 0
    static short last_identify_flag = 0;
    if (*dynamics_identifying_status == 6 && last_identify_flag == 5)
    {
        reportInfo(INFO_WRITE_DYNAMICS_COMPLETE, "writting dynamics complete");
    }
    if (*dynamics_identifying_status != 5)
    {
        last_identify_flag = *dynamics_identifying_status;
        return;
    }
    UINT16 dataCrc = 0;
    // 上电下使能状态才允许操作
    if (!power_on && enbale && *dynamics_identifying_status == 5)
    {
        rtapi_print("Servo Parameter should be set in poweron and disabled state!\n");  // @liwang FIXME 增加错误提示信息
        *dynamics_identifying_status = 4;                                               // 退回辨识结束状态
        reportError(DYNAMIC_SETTING_PROCESS_ABORT, "Dynamcis parameters setting has been unnormally abort");
        return;
    }
    // 启动写参数 @liwang FIXME
    if (last_identify_flag == 4 && *dynamics_identifying_status == 5)
    {
        rtapi_print("Enable parameter writing!\n");
        dataCrc = CheckCRC((UINT32*)servo_dynamics_para, 66);  // 计算CRC
        rtapi_print("Calculate CRC : %d\n", dataCrc);
        // 解参数
        for (int i = 0; i < 6; i++) { enableControlDataWrite(i); }

        // 写动力学版本参数
        for (int i = 0; i < 6; i++) { setRobotDynamicsVersion(i, dynamic_version); }

        // 写CRC参数
        for (int i = 0; i < 6; i++) { setRobotDynamicsCRC(i, dataCrc); }

        // 写动力学参数
        for (int i = 0; i < 60; i++) { setRobotDynamicsPara(i / 10, i % 10 + 2, servo_dynamics_para[i + 6]); }
    }
    last_identify_flag = *dynamics_identifying_status;
#endif
}

void setServoFriction(int power_on,
                      int enbale,
                      UINT8* const cmd_count,
                      void* const set_get_cmd,
                      const int* servo_dynamics_para,
                      int dynamic_version,
                      short* const dynamics_identifying_status)
{
#if 0
    static short last_identify_flag = 0;
    if (*dynamics_identifying_status == 6 && last_identify_flag == 5)
    {
        reportInfo(INFO_WRITE_DYNAMICS_COMPLETE, "writting dynamics complete");
    }
    if (*dynamics_identifying_status != 5)
    {
        last_identify_flag = *dynamics_identifying_status;
        return;
    }
    UINT16 dataCrc = 0;
    // 上电下使能状态才允许操作
    if (!power_on && enbale && *dynamics_identifying_status == 5)
    {
        rtapi_print("Servo Parameter should be set in poweron and disabled state!\n");  // @liwang FIXME 增加错误提示信息
        *dynamics_identifying_status = 4;                                               // 退回辨识结束状态
        reportError(DYNAMIC_SETTING_PROCESS_ABORT, "Dynamcis parameters setting has been unnormally abort");
        return;
    }
    // 启动写参数 @liwang FIXME
    if (last_identify_flag == 4 && *dynamics_identifying_status == 5)
    {
        rtapi_print("Enable parameter writing!\n");
        dataCrc = CheckCRC((UINT32*)servo_dynamics_para, 66);  // 计算CRC
        rtapi_print("Calculate CRC : %d\n", dataCrc);
        // 解参数
        for (int i = 0; i < 6; i++) { enableControlDataWrite(i); }

        // 写动力学版本参数
        for (int i = 0; i < 6; i++) { setRobotDynamicsVersion(i, dynamic_version); }

        // 写CRC参数
        for (int i = 0; i < 6; i++) { setRobotDynamicsCRC(i, dataCrc); }

        // 写动力学参数
        for (int i = 0; i < 60; i++) { setRobotDynamicsPara(i / 10, i % 10 + 2, servo_dynamics_para[i + 6]); }
    }
    last_identify_flag = *dynamics_identifying_status;
#endif
}

// static int query_dynparam_version(RobotParam* const robot_dyn_para, UINT32 max_set_get_num, UINT8* const cmd_count, SetGetCmd* const set_get_cmd)
// {
//     UINT8 current_cmd_num = *cmd_count;
//     int i = 0;
//     if (current_cmd_num >= max_set_get_num - 1)
//     {
//         return 0;
//     }

//     for (i = 0; i < current_cmd_num; i++)
//     {  // check already query
//         if (set_get_cmd[i].cmd_type == QUERY_DYNPARAM_VER)
//         {
//             return 0;
//         }
//     }

//     if (robot_dyn_para->dynParamEna)
//     {
//         set_get_cmd[current_cmd_num].cmd_type = QUERY_DYNPARAM_VER;
//         set_get_cmd[current_cmd_num].send_type = QUERY_SERVO_CMD;
//         set_get_cmd[current_cmd_num].get_data[0] = 1;  // only check the joint 1
//         set_get_cmd[current_cmd_num].get_data[1] = 93;
//         set_get_cmd[current_cmd_num].get_data[2] = 1;
//         initSetData(&set_get_cmd[current_cmd_num]);
//         ++current_cmd_num;
//     }
//     *cmd_count = current_cmd_num;
//     return 0;
// }

// static int query_dynparam_data(RobotParam* const robot_dyn_para, UINT32 max_set_get_num, UINT8* const cmd_count, SetGetCmd* const set_get_cmd)
// {
//     UINT8 current_cmd_num = *cmd_count;
//     int i = 0, k = 0;
//     if (current_cmd_num >= max_set_get_num - 61)
//     {  // crc and data
//         return 0;
//     }

//     for (i = 0; i < current_cmd_num; i++)
//     {  // check already query
//         for (k = 0; k < 10; k++)
//         {
//             if (set_get_cmd[i].cmd_type == QUERY_DYNPARAM_GROUP0 + 2 * k)
//                 return 0;
//         }
//     }

//     // 必须等于版本号, @FIXME
//     if (robot_dyn_para->dynParamData[0] == 0x01)
//     {
//         set_get_cmd[current_cmd_num].cmd_type = QUERY_DYNPARAM_CRC;
//         set_get_cmd[current_cmd_num].send_type = QUERY_SERVO_CMD;
//         set_get_cmd[current_cmd_num].get_data[0] = 1;  // only check the joint 1
//         set_get_cmd[current_cmd_num].get_data[1] = 93;
//         set_get_cmd[current_cmd_num].get_data[2] = 0;
//         initSetData(&set_get_cmd[current_cmd_num]);
//         ++current_cmd_num;

//         for (k = 0; k < 10; k++)
//         {
//             for (i = 0; i < 6; i++)
//             {
//                 set_get_cmd[i + current_cmd_num].cmd_type = QUERY_DYNPARAM_GROUP0 + 2 * k;
//                 set_get_cmd[i + current_cmd_num].send_type = QUERY_SERVO_CMD;
//                 set_get_cmd[i + current_cmd_num].get_data[0] = i + 1;
//                 set_get_cmd[i + current_cmd_num].get_data[1] = 93;
//                 set_get_cmd[i + current_cmd_num].get_data[2] = 02 + k;
//                 initSetData(&set_get_cmd[i + current_cmd_num]);
//             }
//             current_cmd_num += 6;
//         }
//     }
//     else
//     {
//         robot_dyn_para->dynParamState = 3;  // dyn version invalid
//         reportError(SERVO_DYNPARAM_VER_UNSUPPORTED, "unsupported dynamics parameter version: %d\n", robot_dyn_para->dynParamData[0]);
//     }

//     *cmd_count = current_cmd_num;
//     return 0;
// }

// void processQuerryDyanmcis(const int i,
//                            const int joint,
//                            const VCI_CAN_OBJ* const rx_message,
//                            RobotParam* const robot_dyn_para,
//                            UINT32 max_set_get_num,
//                            UINT8* const cmd_count,
//                            SetGetCmd* const set_get_cmd)
// {
//     if ((rx_message[i].Data[1] == 10) && (rx_message[i].Data[2] == 6))
//     {
//         delete_set_get_cmd(QUERY_DYNPARAM_ENA, 0x0A06, joint + 1);
//         robot_dyn_para->dynParamEna = rx_message[i].Data[6] + (rx_message[i].Data[5] << 8) + (rx_message[i].Data[4] << 16) + (rx_message[i].Data[3] << 24);
//         // rtapi_print("QUERY_DYNPARAM_ENA obtained: %x\n", robot_dyn_para->dynParamEna);
//         if (robot_dyn_para->dynParamEna == 0x5555)
//         {
//             // 动力学参数使能，请求版本，仅请求1轴
//             query_dynparam_version(robot_dyn_para, max_set_get_num);
//         }
//         else
//         {
//             robot_dyn_para->dynParamState = 1;  // not enabled
//         }
//     }
//     else if ((rx_message[i].Data[1] == 93) && (rx_message[i].Data[2] == 1))
//     {
//         delete_set_get_cmd(QUERY_DYNPARAM_VER, 0x5D01, joint + 1);
//         robot_dyn_para->dynParamData[0] = rx_message[i].Data[6] + (rx_message[i].Data[5] << 8) + (rx_message[i].Data[4] << 16) + (rx_message[i].Data[3] << 24);
//         robot_dyn_para->dynParamData[5] = robot_dyn_para->dynParamData[4] = robot_dyn_para->dynParamData[3] = robot_dyn_para->dynParamData[2] =
//             robot_dyn_para->dynParamData[1] = robot_dyn_para->dynParamData[0];
//         rtapi_print("QUERY_DYNPARAM_VER obtained: %x\n", robot_dyn_para->dynParamData[0]);

//         // 动力学在线辨识
//         if (zucmotStatus->dynamics_identifying_status == 5 || zucmotStatus->friction_identifying_status == 5)  // 如果正在写动力学参数，不必再query
//         {
//             // 记录回传数据，用于校验
//             dynamcis_setting_check[joint + DYNAMIC_VERSION_OFFSET] = robot_dyn_para->dynParamData[0];
//             //
//             delete_set_get_cmd(SET_ROBOT_DYNAMICS_VERSION, 0x5D01, joint + 1);
//         }
//         else
//         {
//             query_dynparam_data(robot_dyn_para, max_set_get_num);
//         }
//     }
//     else if ((rx_message[i].Data[1] == 93) && (rx_message[i].Data[2] == 0))
//     {
//         delete_set_get_cmd(QUERY_DYNPARAM_CRC, 0x5D00, joint + 1);
//         robot_dyn_para->dynCRC = rx_message[i].Data[6] + (rx_message[i].Data[5] << 8) + (rx_message[i].Data[4] << 16) + (rx_message[i].Data[3] << 24);
//         rtapi_print("QUERY_DYNPARAM_CRC obtained: %d\n", robot_dyn_para->dynCRC);

//         // 动力学在线辨识
//         if (zucmotStatus->dynamics_identifying_status == 5 || zucmotStatus->friction_identifying_status == 5)  // 如果正在写动力学参数，不必再query
//         {
//             // 记录回传数据，用于校验
//             dynamcis_setting_check[joint + DYNAMIC_CRC_OFFSET] = robot_dyn_para->dynCRC;
//             //
//             delete_set_get_cmd(SET_ROBOT_DYNAMICS_CRC, 0x5D00, joint + 1);
//         }
//     }
//     else if ((rx_message[i].Data[1] == 93) &&
//              (rx_message[i].Data[2] == 2 || rx_message[i].Data[2] == 3 || rx_message[i].Data[2] == 4 || rx_message[i].Data[2] == 5 ||
//               rx_message[i].Data[2] == 6 || rx_message[i].Data[2] == 7 || rx_message[i].Data[2] == 8 || rx_message[i].Data[2] == 9 ||
//               rx_message[i].Data[2] == 10 || rx_message[i].Data[2] == 11))
//     {
//         int offset = rx_message[i].Data[2] - 2;
//         int paramID = (rx_message[i].Data[1] << 8) + rx_message[i].Data[2];
//         delete_set_get_cmd(QUERY_DYNPARAM_GROUP0 + 2 * offset, paramID, +1);

//         robot_dyn_para->dynParamData[10 * joint + offset + 6] =
//             rx_message[i].Data[6] + (rx_message[i].Data[5] << 8) + (rx_message[i].Data[4] << 16) + (rx_message[i].Data[3] << 24);
//         robot_dyn_para->dynParamReadyNum++;
//         rtapi_print("QUERY_DYNPARAM_PARAM obtained: %d\n", robot_dyn_para->dynParamReadyNum);

//         if (robot_dyn_para->dynParamReadyNum == 60)
//         {
//             if (checkrobot_dyn_paraCRC(1) == 0)
//             {
//                 robot_dyn_para->dynParamState = 2;  // ready
//             }
//             else
//             {
//                 robot_dyn_para->dynParamState = 3;  // crc error
//                 reportError(SERVO_DYNPARAM_CRC_ERROR, "invalid CRC of dynamics parameters from servo side\n");
//             }
//         }

//         // 动力学在线辨识
//         if (zucmotStatus->dynamics_identifying_status == 5 || zucmotStatus->friction_identifying_status == 5)
//         {
//             rtapi_print("QUERY_DYNPARAM_PARAM obtained: %d, %d\n", 10 * joint + offset, robot_dyn_para->dynParamData[10 * joint + offset + 6]);
//             dynamcis_setting_check[10 * joint + offset + DYNAMIC_PARA_NUM_OFFSET] =
//                 rx_message[i].Data[6] + (rx_message[i].Data[5] << 8) + (rx_message[i].Data[4] << 16) + (rx_message[i].Data[3] << 24);

//             delete_set_get_cmd(SET_ROBOT_DYNAMICS_PARA1 + offset, paramID, joint + 1);
//             if (10 * joint + offset == 59)
//             {
//                 // 动力学参数写完毕,关锁
//                 for (int i = 0; i < 6; i++) { disableControlDataWrite(i); }
//             }
//         }
//     }
// }